
/*
告诉海龟位置让让他自己去找
1 将当前坐标作差得到距离，将角度作差得到方向

*/
/*
告诉海龟位置让让他自己去找
1 将当前坐标作差得到距离，将角度作差得到方向

*/
# include "rclcpp/rclcpp.hpp"
# include "geometry_msgs/msg/twist.hpp"
# include "turtlesim/msg/pose.hpp"

using namespace std::chrono_literals;


class TurtleControlNode :public rclcpp::Node{

    private:
        rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_;
        rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr sub_;
        double target_x{1.0};
        double target_y{1.0};
        double k_{1.0};
        double max_speed_{2.0};
    public:
        TurtleControlNode(const std::string & node_name):Node(node_name){
            RCLCPP_INFO(this->get_logger(),"话题启动");
            pub_ = this->create_publisher<geometry_msgs::msg::Twist>("turtle1/cmd_vel",10);  // 话题名词不能随便
            // timer_ = this->create_wall_timer(std::chrono::milliseconds(500),std::bind(&TurtleControlNode::timer_callback,this));
            sub_ = this->create_subscription<turtlesim::msg::Pose>("turtle1/pose",10,std::bind(&TurtleControlNode::sub_callback,this,std::placeholders::_1));
        }

        // void timer_callback(){
        //     auto msg = geometry_msgs::msg::Twist();
        //     msg.linear.x = 1.0;
        // }
        void sub_callback(const turtlesim::msg::Pose::SharedPtr pose){
            // std::cout << "请输入目标位置和角度（用空格分隔）：" << std::endl;
            // std::cin >> target_x >> target_y >> target_theta;

            // if (std::cin.fail()) {
            //     std::cerr << "输入错误，请输入数字。" << std::endl;
            //     std::cin.clear(); // 清除错误标志
            //     std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n'); // 忽略剩余输入
            //     return;
            // }

            double current_x = pose->x;
            double current_y = pose->y;
            RCLCPP_INFO(this->get_logger(),"当前位置：(%.2f,%.2f)",current_x,current_y);

            auto distance = std::sqrt(std::pow(target_x - current_x, 2) + std::pow(target_y - current_y, 2));
            auto angle = std::atan2(target_y - current_y, target_x - current_x) - pose->theta;
            auto msg = geometry_msgs::msg::Twist();

            if(distance > 0.1){
                if(fabs(angle)>0.2){
                    msg.angular.z = fabs(angle);
                }else{
                    msg.linear.x = k_ * distance;

                }
            }

            if(msg.linear.x > max_speed_){
                msg.linear.x = max_speed_;
            }

            

            // 计算目标位置与当前位置的差值
        
        
            pub_->publish(msg);

        }
};
int main(int argc,char ** argv){
    rclcpp::init(argc,argv);
    rclcpp::spin(std::make_shared<TurtleControlNode>("turtle_control"));
    rclcpp::shutdown();
    return 0;
}

